/*
 * Tasks:
 * Method that passes in desired angle
 * Add PID loop to make wrist go to specific angle
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Solenoid;
import java.util.TimerTask;

/**
 *
 * @author buttercream
 */
public class Wrist extends TimerTask
{
    private static double ADJUST_THRESHHOLD = 0.1;

    private Joystick joyStick;
    private PIDController pid;

    private Solenoid claw;
    private MagneticEnc encoder;
    private Jaguar motor;

    boolean calibrated = false;

    private static boolean CLAW_OPEN = true;
    private static boolean CLAW_CLOSE = false;
    private static boolean CLAW_INITIAL = CLAW_OPEN;

    public Wrist( Joystick joyStick )
    {
        this.joyStick = joyStick;

        encoder = new MagneticEnc( Constants.WRIST_CHNL  );
        motor = new Jaguar(Constants.CRIO_DIGITAL_SIDECAR, Constants.WRIST_MOTOR);
        pid = new PIDController( Constants.WRIST_PID_P, Constants.WRIST_PID_I,
                 Constants.WRIST_PID_D, encoder, motor);
        claw = new Solenoid(Constants.CRIO_PNEUMATICS, Constants.CLAW_SOLENOID);
//         pid.setTolerance(Constants.PID_TOLERANCE);
//         pid.setInputRange(-1, 1);
//         pid.setContinuous();
        pid.enable();

        claw.set( CLAW_INITIAL );
    }

    public void run(  )
    {
        //Be sure to wait until calibration is finished before allowing arm
        //to do anything!
        if ( !calibrated )
        {
            goToPosition( Constants.WRIST_NEUTRAL );

            if(pid.onTarget()) {
                calibrated = true;
            }
        }
        else
        {
            getCommandFromJoystick();
        }
        //Check to see if a button is pressed. If it is, figure out what level
        //to move it to. Set PID to go to that height.
        //Remember to consider that a joystick button may be pressed through
        //multiple cycles. Won't necessarily be a single press in a single
        //iteration

        //Run this every 10 ms
        //Set PID controller to joystick value (could be neutral or angle)


    }

    private void getCommandFromJoystick()
    {
//        if ( joyStick.getRawButton(  BTN_LIFTER_TOP ) )
//        else if(joyStick.getRawButton(BTN_LIFTER_BOTTOM))
        if      ( isButtonPressed( Constants.BTN_FLOOR ))
        {
            floor();
        }
        else if ( isButtonPressed( Constants.BTN_TUCKED ))
        {
            tucked();
        }
        else if ( isButtonPressed( Constants.BTN_TOP_ROW_SIDE )  ||
                  isButtonPressed( Constants.BTN_TOP_ROW_CENTER )  )
        {
            topRow();
        }
        else if ( isButtonPressed( Constants.BTN_MIDDLE_ROW_SIDE )   ||
                  isButtonPressed( Constants.BTN_MIDDLE_ROW_CENTER )   )
        {
            middleRow();
        }
        else if ( isButtonPressed( Constants.BTN_BOTTOM_ROW_SIDE ) ||
                  isButtonPressed( Constants.BTN_BOTTOM_ROW_CENTER )
                  )
        {
            bottomRow();
        }


        else manualAdjust();
    }

    private boolean isButtonPressed( int button )
    {
        return joyStick.getRawButton( button );
    }

    private void goToPosition( double position )
    {
        pid.setSetpoint(position);
    }

    public void floor()
    {
        goToPosition( Constants.WRIST_FLOOR );
    }

    public void tucked()
    {
        goToPosition( Constants.WRIST_TUCKED );
    }

    public void topRow()
    {
        goToPosition( Constants.WRIST_TOP_ROW );
    }

    public void middleRow()
    {
        goToPosition( Constants.WRIST_MIDDLE_ROW );
    }

    public void bottomRow()
    {
        goToPosition( Constants.WRIST_BOTTOM_ROW);
    }

    public void openClaw()
    {
        claw.set( CLAW_OPEN );
    }

    public void closeClaw()
    {
        claw.set( CLAW_CLOSE );
    }

    private void manualAdjust()
    {
        if(joyStick.getRawButton(Constants.BTN_WRIST_MANUAL_UP)) {
            Utils.pauseUntilReleased(joyStick, Constants.BTN_WRIST_MANUAL_UP);
            double position = pid.getSetpoint();
            pid.setSetpoint(position + 1);
        }
        else if(joyStick.getRawButton(Constants.BTN_WRIST_MANUAL_DOWN)) {
            Utils.pauseUntilReleased(joyStick, Constants.BTN_WRIST_MANUAL_DOWN);
            double position = pid.getSetpoint();
            pid.setSetpoint(position - 1);
        }
        else if ( joyStick.getRawButton(Constants.BTN_CLAW_GRAB) &&
                joyStick.getRawButton(Constants.BTN_CLAW_RELEASE_WITH_GRAB ) )
        {
            closeClaw();
        }
        else if ( joyStick.getRawButton(Constants.BTN_CLAW_GRAB) )
        {
            openClaw();
        }
    }
}
